﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using System.Threading;
using System.Diagnostics;
using MotionPlanning.MotionPlanningPrimitives;

namespace MotionPlanning.MotionPlanningComponents
{
    public class PointSetComponent
    {
        protected static int MAX_DIMENSIONS = 6;
        protected int DOF_coordinate;
        protected double[] q_max;
        protected double[] q_min;
        protected int[] q_topology;
        protected double[] q_scale;
        protected Cell root;
        protected int Depth;
        protected int MaxCellCapacity;
        protected int MAX_NEIGHBORS;
        protected int Key;
        protected double dist_safezone;

        static public Hashtable NodeToIndexOnTrain(List<object> train)
        {
            Hashtable hashTable = new Hashtable(train.Count);
            for (int i = 0; i < train.Count; ++i)
            {
                hashTable.Add(train[i], i);
            }
            return hashTable;
        }
        
        public PointSetComponent(int DOF, double[] q_max, double[] q_min, double[] q_scale, int[] q_topology, int preConstructedLevel, bool NeighborGraph)
        {
            Key = 0;
            DOF_coordinate = DOF;
            MAX_NEIGHBORS = (int)Math.Pow(3, DOF) - 1;
            this.q_max = q_max;
            this.q_min = q_min;
            this.q_topology = q_topology;
            this.q_scale = q_scale;
            Depth = preConstructedLevel;
            root = new Cell(0, q_min, q_max, 0, this);
            root.PreConstructToLevel();
            if(NeighborGraph == true)
            {
                if (DOF_coordinate <= MAX_DIMENSIONS)
                    root.BuildNeighborGraph();
                else
                    throw new Exception("dimension larger than " + MAX_DIMENSIONS + " is too expensive to build neighborhood graph");
            }
        }

        public int GetNeighborsNumber()
        {
            return MAX_NEIGHBORS;
        }

        public int GetDepth()
        {
            return this.Depth;
        }

        public Cell Testing_GetThisCell(double[] q)
        {
            Cell targetCell = null;
            this.root.GetDeepestCell(q, ref targetCell);
            return targetCell;
        }

        /*
         * internal version of distance function 
         * only make sense locally
         * does not taken topology into consideration
         * scale is used to compute square sum
         */
        private double LocalWeightedEuclidean2(double[] c1, double[] c2)
        {
            double squareSum = 0;
            for (int i = 0; i < DOF_coordinate; ++i)
            {
                double diff = (c1[i] - c2[i]) * q_scale[i];
                squareSum += diff * diff;
            }
            return squareSum;
        }

        public object Contains(double[] coordinate)
        {
            Cell targetCell = null;
            root.GetDeepestCell(coordinate, ref targetCell);
            return targetCell.HasPoint(coordinate);
        }

        public bool AddPoint(double[] key, Object pointObject)
        {
            for (int i = 0; i < DOF_coordinate; ++i)
            {
                if (q_topology[i] != 1) continue;   //1 indicates euclidean
                if (key[i] < this.q_min[i]) return false;
                if (key[i] > this.q_max[i]) return false;
            }
            root.AddPoint(key, pointObject);
            return true;
        }

        public bool DelPoint(double[] key, Object pointObject)
        {
            root.DelPoint(key, pointObject);
            return true;
        }

        public void NearestNeighbor_Approximate(double[] key, ref double distance, ref Object pointObject)
        {
            Cell centerCell = root.CenterCell(key);
            centerCell.NearestNeighbor_SubTree(key, ref distance, ref pointObject);
        }

        public void NearestNeighbor_Exact(double[] key, ref double distance, ref Object pointObject)
        {
            throw (new Exception("Function unimplemented."));
        }

        public void NearestNeighbors_WithinRange_Exclusive(double[] key, double delta, ref List<object> nodeList)
        {
            List<object> train = this.GetAllDataObjects_possibleWithinRange(key, delta);
            if (nodeList == null)
                nodeList = new List<object>();
            for (int i = 0; i < train.Count; ++i)
            {
                Node cur_x = (Node)train[i];
                double[] cur_q = cur_x.get_Value().get_q();
                double dist2 = LocalWeightedEuclidean2(key, cur_q);
                if (dist2 <= delta * delta)
                    nodeList.Add(cur_x);
            }
        }

        public void NearestNeighbors_WithinRange_Expansive(double[] key, double delta, ref List<object> nodeList)
        {
            throw new Exception("Function obsolete");

            ++Key;
            if (nodeList == null) nodeList = new List<object>();
            Cell targetCell = null;
            root.GetDeepestCell(key, ref targetCell);
            LinkedList<Cell> fringe = new LinkedList<Cell>();
            fringe.AddLast(targetCell);
            double[] coordinate_cellCenter=new double[targetCell.lowBounds.Length];
            while(fringe.Count>0)
            {
                //pop from fringe
                targetCell = fringe.First.Value;
                fringe.RemoveFirst();
                if (targetCell.Key == this.Key)
                    continue;
                targetCell.Key = this.Key;  //marked as visited
                for (int i = 0; i < targetCell.PointList.Count; ++i)
                {
                    double[] q = targetCell.PointList[i].Key;
                    double dist2 = LocalWeightedEuclidean2(key, q);
                    if (dist2 <= delta * delta)
                        nodeList.Add(targetCell.PointList[i].Value);
                }
                //insert children
                dist_safezone = double.MaxValue;
                for (int i = 0; i < coordinate_cellCenter.Length; ++i)
                {
                    coordinate_cellCenter[i] = (targetCell.highBounds[i] + targetCell.lowBounds[i]) * 0.5;
                    double iLength = Math.Abs(targetCell.highBounds[i] - targetCell.lowBounds[i]);
                    if (dist_safezone > iLength)
                        dist_safezone = iLength;
                }
                double dist_tmp = Math.Sqrt(this.LocalWeightedEuclidean2(coordinate_cellCenter, key));
                double delta_ext = delta + dist_safezone;
                if (dist_tmp * dist_tmp > delta_ext * delta_ext) continue;
                for (int i = 0; i < targetCell.neighbors.Count; ++i)
                {
                    Cell childCell = targetCell.neighbors[i];
                    if (childCell.Key == this.Key) continue;
                    fringe.AddLast(childCell);
                }

            }
        }
        
        public void NearestNeighbors_KClosest_Exclusive(double[] key, int K, ref List<object> nodeList)
        {
            if (K < 0) throw new Exception("K cannot be negative");

            if (root.PointCount < K)
                K = root.PointCount;

            double iRadius = 0.1;
            nodeList = new List<object>();
            while (nodeList.Count < K)
            {
                nodeList = new List<object>();
                NearestNeighbors_WithinRange_Exclusive(key, iRadius, ref nodeList);
                iRadius *= 2;
            }
            BinaryHeap minHeap = new BinaryHeap(nodeList.Count);
            for (int i = 0; i < nodeList.Count; ++i)
            {
                Node n = (Node)nodeList[i];
                minHeap.Push(this.LocalWeightedEuclidean2(n.get_Value().get_q(), key), nodeList[i]);
            }
            nodeList = new List<object>();
            for (int i = 0; i < K; ++i)
            {
                double value = 0;
                object data = null;
                minHeap.Pop(ref value, ref data);
                nodeList.Add(data);
            }
        }

        public void NearestNeighbors_KClosest_Expansive(double[] key, int K, ref List<object> nodeList)
        {
            throw new Exception("Function obsolete");

            if (K < 0) throw new Exception("K cannot be negative");
            double iRadius = 0.1;
            nodeList = new List<object>();
            while (nodeList.Count < K)
            {
                nodeList = new List<object>();
                NearestNeighbors_WithinRange_Expansive(key, iRadius, ref nodeList);
                iRadius *= 2;
            }
            BinaryHeap minHeap = new BinaryHeap(nodeList.Count);
            for (int i = 0; i < nodeList.Count; ++i)
            {
                Node n = (Node)nodeList[i];
                minHeap.Push(this.LocalWeightedEuclidean2(n.get_Value().get_q(), key), nodeList[i]);
            }
            nodeList = new List<object>();
            for (int i = 0; i < K; ++i)
            {
                double value = 0;
                object data = null;
                minHeap.Pop(ref value, ref data);
                nodeList.Add(data);
            }
        }

        public List<object> GetAllDataObjects()
        {
            List<object> train = new List<object>();
            root.ReportContent(ref train);
            return train;
        }

        public List<object> GetAllDataObjects_possibleWithinRange(double[] key, double radius)
        {
            List<object> train = new List<object>();
            root.ReportContent_possibleWithinRange(key, radius, ref train);
            return train;
        }

        public void EmptyAllDataObjects()
        {
            root.EmptyPoints();
        }

        public int GetSize()
        {
            return root.PointCount;
        }

        public class BinaryHeap
        {
            private int Count;
            private double[] keyArray;
            private List<object> dataArray;

            public BinaryHeap(int size)
            {
                if (size < 0) throw new Exception("size of BinaryHeap cannot be negative");
                keyArray = new double[size];
                dataArray = new List<object>(size);
                for (int i = 0; i < size; ++i)
                    dataArray.Add(null);
                Count = 0;
            }

            public int GetCapacity()
            {
                return keyArray.Length;
            }

            public bool Pop(ref double key, ref object data)
            {
                if (Count <= 0) return false;

                key = keyArray[0];
                data = dataArray[0];

                keyArray[0] = keyArray[Count - 1];
                dataArray[0] = dataArray[Count - 1];

                keyArray[Count - 1] = double.MaxValue;
                dataArray[Count - 1] = null;

                Count--;

                int i = 0;
                while (i < Count - 1)
                {
                    int childIdx = GetChildIndex(i, 0);
                    if (childIdx > Count - 1) break;
                    int childIdx_tmp = GetChildIndex(i, 1);
                    if (childIdx_tmp < Count) 
                        if (keyArray[childIdx] > keyArray[childIdx_tmp])
                            childIdx = childIdx_tmp;

                    if (keyArray[i] <= keyArray[childIdx])
                        break;
                    //swap
                    double key_tmp = keyArray[childIdx];
                    keyArray[childIdx] = keyArray[i];
                    keyArray[i] = key_tmp;
                    object data_tmp = dataArray[childIdx];
                    dataArray[childIdx] = dataArray[i];
                    dataArray[i] = data_tmp;
                    //goto child
                    i = childIdx;
                }
                return true;
            }

            public bool Push(double key, object data)
            {
                if (Count >= keyArray.Length)
                    return false;
                //add to the bottom of the heap
                int i = Count;
                keyArray[i] = key;
                dataArray[i] = data;
                Count++;
                int j = GetParentIndex(i);
                while (j < i)
                {
                    if (keyArray[j] <= keyArray[i])
                        break;
                    //swap
                    double key_tmp = keyArray[j];
                    keyArray[j] = keyArray[i];
                    keyArray[i] = key_tmp;
                    object data_tmp = dataArray[j];
                    dataArray[j] = dataArray[i];
                    dataArray[i] = data_tmp;
                    //goto parent
                    i = j;
                    j = GetParentIndex(i);
                }
                return true;
            }

            private int GetParentIndex(int index)
            {
                if (index == 0) return 0;
                if(index < 0 ) throw new Exception("index cannot be negative in binaryHeap");
                return (int)Math.Floor((index - 1) / 2.0);
            }

            private int GetChildIndex(int index, int isRightChild)
            {
                if (index < 0) throw new Exception("index cannot be negative in binaryHeap");
                if (isRightChild < 0 || isRightChild > 1) throw new Exception("isRightChild must be zero or one");

                return index * 2 + 1 + isRightChild;
            }
        }

        public class Cell
        {
            protected ReaderWriterLockSlim rwLocker;
            public int PointCount;
            public int curLevel;
            public int iAxis;
            public double[] lowBounds;
            public double[] highBounds;
            public Cell LeftTree;
            public Cell RightTree;
            public Cell ParentTree;
            public List<KeyValuePair<double[], Object>> PointList;
            public List<Cell> neighbors;
            public PointSetComponent Host;
            public int Key;

            public Cell(int pAxis, double[] lowB, double[] highB, int myLevel, PointSetComponent host)
            {
                curLevel = myLevel;
                iAxis = pAxis;
                lowBounds = lowB;
                highBounds = highB;
                LeftTree = null;
                RightTree = null;
                ParentTree = null;
                PointList = null;
                Host = host;
                neighbors = null;
                rwLocker = null;
                Key = 0;
            }

            public void BuildNeighborGraph()
            {
                if (curLevel < Host.Depth)
                {
                    LeftTree.BuildNeighborGraph();
                    RightTree.BuildNeighborGraph();
                }
                else
                {
                    FindMyNeighbors();
                }
            }

            public void FindMyNeighbors()
            {
                if (curLevel < Host.Depth)
                {
                    throw new Exception("this should not happen. curLevel should be the bottom level");
                }
                else
                {
                    this.neighbors = new List<Cell>(this.Host.GetNeighborsNumber());
                    double[] cellCenter = new double[lowBounds.Length];
                    for (int i = 0; i<cellCenter.Length;++i )
                        cellCenter[i] = (highBounds[i] + lowBounds[i]) * 0.5;
                    DimensionalTraversal(0, cellCenter, this);
                }
            }

            //this one should be private
            public void DimensionalTraversal(int iAxis, double[] cellCenter, Cell initCell)
            {
                double[] curCellCenter = new double[cellCenter.Length];
                for (int i = 0; i < curCellCenter.Length; ++i)
                    curCellCenter[i] = cellCenter[i];
                if (iAxis < lowBounds.Length)
                {
                    double iCellLength = Math.Abs(highBounds[iAxis] - lowBounds[iAxis]);
                    // small neighbor
                    curCellCenter[iAxis] -= iCellLength;
                    DimensionalTraversal(iAxis + 1, curCellCenter, initCell);
                    // same neighbor
                    curCellCenter[iAxis] += iCellLength;
                    DimensionalTraversal(iAxis + 1, curCellCenter, initCell);
                    // big neighbor
                    curCellCenter[iAxis] += iCellLength;
                    DimensionalTraversal(iAxis + 1, curCellCenter, initCell);
                }
                else
                {
                    Cell neighborCell = null;
                    Host.root.GetDeepestCell(curCellCenter, ref neighborCell);
                    if (neighborCell != initCell)
                        neighbors.Add(neighborCell);
                    //else
                        //Debugger.Break();
                }
            }

            public void PreConstructToLevel()
            {
                if (curLevel < Host.Depth)
                {
                    int nAxis = iAxis + 1;
                    if (nAxis >= Host.DOF_coordinate) nAxis = 0;
                    double midv = (lowBounds[iAxis] + highBounds[iAxis]) * 0.5;

                    double[] L_nlB = new double[Host.DOF_coordinate];
                    double[] L_nhB = new double[Host.DOF_coordinate];
                    double[] R_nlB = new double[Host.DOF_coordinate];
                    double[] R_nhB = new double[Host.DOF_coordinate];
                    for (int i = 0; i < Host.DOF_coordinate; ++i)
                    {
                        L_nlB[i] = lowBounds[i];
                        L_nhB[i] = highBounds[i];
                        R_nlB[i] = lowBounds[i];
                        R_nhB[i] = highBounds[i];
                    }
                    L_nhB[iAxis] = midv;
                    R_nlB[iAxis] = midv;
                    LeftTree = new Cell(nAxis, L_nlB, L_nhB, curLevel + 1, Host);
                    LeftTree.ParentTree = this;
                    RightTree = new Cell(nAxis, R_nlB, R_nhB, curLevel + 1, Host);
                    RightTree.ParentTree = this;
                    LeftTree.PreConstructToLevel();
                    RightTree.PreConstructToLevel();
                }
                else
                {
                    this.PointList = new List<KeyValuePair<double[], object>>();
                    rwLocker = new ReaderWriterLockSlim();
                }
            }

            public void EmptyPoints()
            {
                if (curLevel < Host.Depth)
                {
                    RightTree.EmptyPoints();
                    LeftTree.EmptyPoints();
                    PointCount = LeftTree.PointCount + RightTree.PointCount;
                }
                else if (curLevel == Host.Depth)
                {
                    rwLocker.EnterWriteLock();
                    try
                    {
                        PointList = new List<KeyValuePair<double[], object>>();
                        PointCount = PointList.Count;
                    }
                    finally
                    {
                        rwLocker.ExitWriteLock();
                    }
                }
            }

            public void AddPoint(double[] coordinate, Object content)
            {
                if (curLevel < Host.Depth)
                {
                    double midv = (lowBounds[iAxis] + highBounds[iAxis]) * 0.5;
                    double ckey = coordinate[iAxis];
                    if (ckey >= midv)
                        RightTree.AddPoint(coordinate, content);
                    else
                        LeftTree.AddPoint(coordinate, content);
                    PointCount = LeftTree.PointCount + RightTree.PointCount;
                }
                else if (curLevel == Host.Depth)
                {
                    rwLocker.EnterWriteLock();
                    try
                    {

                        if (PointList == null)
                            PointList = new List<KeyValuePair<double[], object>>();
                        PointList.Add(new KeyValuePair<double[], object>(coordinate, content));
                        PointCount = PointList.Count;

                        if (PointCount > Host.MaxCellCapacity)
                        {
                            //Properties.MaxCellCapacity = Count;
                            //System.Diagnostics.Debugger.Break();
                        }
                    }
                    finally
                    {
                        rwLocker.ExitWriteLock();
                    }
                }
                else
                    throw (new Exception("Trying to access level beyond Depth in AddPoint"));
            }

            public void DelPoint(double[] coordinate, Object content)
            {
                if (curLevel < Host.Depth)
                {
                    double midv = (lowBounds[iAxis] + highBounds[iAxis]) / 2;
                    double ckey = coordinate[iAxis];
                    if (ckey >= midv)
                        RightTree.DelPoint(coordinate, content);
                    else
                        LeftTree.DelPoint(coordinate, content);
                    PointCount = LeftTree.PointCount + RightTree.PointCount;
                }
                else if (curLevel == Host.Depth)
                {
                    rwLocker.EnterWriteLock();
                    try
                    {
                        int listCount = this.PointList.Count;
                        for (int i = 0; i < listCount; ++i)
                        {
                            if (this.PointList[i].Value == content)
                            {
                                this.PointList[i] = PointList[listCount - 1];
                                this.PointList.RemoveAt(listCount - 1);
                                break;
                            }
                        }
                        PointCount = this.PointList.Count;
                    }
                    finally
                    {
                        rwLocker.ExitWriteLock();
                    }
                }
                else
                    throw (new Exception("Trying to access level beyond Depth in DelPoint"));
            }

            public object HasPoint(double[] coordinate)
            {
                if (curLevel < Host.Depth)
                {
                    double midv = (lowBounds[iAxis] + highBounds[iAxis]) * 0.5;
                    double ckey = coordinate[iAxis];
                    if (ckey >= midv)
                        return RightTree.HasPoint(coordinate);
                    else
                        return LeftTree.HasPoint(coordinate);
                }
                else if (curLevel == Host.Depth)
                {
                    rwLocker.EnterReadLock();
                    try
                    {
                        if (PointList == null)
                            return null;
                        for (int i = 0; i < this.PointList.Count; ++i)
                        {
                            double[] key_i = PointList[i].Key;
                            int i_match = 0;
                            for (; i_match < key_i.Length; ++i_match)
                            {
                                if (coordinate[i_match] != key_i[i_match])
                                    break;
                            }
                            if (i_match == key_i.Length)
                                return PointList[i].Value;
                        }
                        return null;
                    }
                    finally
                    {
                        rwLocker.ExitReadLock();
                    }
                }
                else
                {
                    throw new Exception();
                }
            }

            /*
             * points outside the box bounded space are in the outer cells
             * will return big cells if cannot searching deeper
             * this function reduces the cells that needed to be searched within
             */
            public Cell CenterCell(double[] key)
            {
                if (curLevel < Host.Depth)
                {
                    double midv = (lowBounds[iAxis] + highBounds[iAxis]) / 2;
                    double ckey = key[iAxis];
                    if (ckey >= midv)
                        if (RightTree.PointCount == 0)
                            return this;
                        else
                            return RightTree.CenterCell(key);
                    else
                        if (LeftTree.PointCount == 0)
                            return this;
                        else
                            return LeftTree.CenterCell(key);
                }
                else if (curLevel == Host.Depth)
                {
                    if (PointCount > 0)
                        return this;
                    else
                        return null;
                }
                else
                    throw (new Exception("The key point beyond Depth in CenterCell"));
            }

            public void GetDeepestCell(double[] key, ref Cell targetCell)
            {
                if (curLevel < Host.Depth)
                {
                    double midv = (lowBounds[iAxis] + highBounds[iAxis]) / 2;
                    double ckey = key[iAxis];
                    if (ckey >= midv)
                        RightTree.GetDeepestCell(key, ref targetCell);
                    else
                        LeftTree.GetDeepestCell(key, ref targetCell);
                }
                else if (curLevel == Host.Depth)
                {
                    targetCell = this;
                }
            }

            /*
             * the function is a hieratical linear search
             */
            public void NearestNeighbor_SubTree(double[] key, ref double best, ref object winner)
            {
                if (curLevel < Host.Depth)
                {
                    RightTree.NearestNeighbor_SubTree(key, ref best, ref winner);
                    LeftTree.NearestNeighbor_SubTree(key, ref best, ref winner);
                }
                else
                {
                    rwLocker.EnterReadLock();
                    try
                    {
                        for (int i = 0; i < PointList.Count; ++i)
                        {
                            double iDistance2 = Host.LocalWeightedEuclidean2(key, PointList[i].Key);
                            if (iDistance2 < best)
                            {
                                best = iDistance2;
                                winner = PointList[i].Value; ;
                            }
                        }
                    }
                    finally
                    {
                        rwLocker.ExitReadLock();
                    }
                }
            }

            public void ReportContent(ref List<object> train)
            {
                if (curLevel < Host.Depth)
                {
                    RightTree.ReportContent(ref train);
                    LeftTree.ReportContent(ref train);
                }
                else
                {
                    rwLocker.EnterReadLock();
                    try
                    {
                        for (int i = 0; i < PointList.Count; ++i)
                        {
                            train.Add(PointList[i].Value);
                        }
                    }
                    finally
                    {
                        rwLocker.ExitReadLock();
                    }
                }
            }

            public void ReportContent_possibleWithinRange(double[] key, double radius, ref List<object> train)
            {
                double hd = highBounds[iAxis] - key[iAxis];
                double ld = lowBounds[iAxis] - key[iAxis];
                if (hd * ld > 0)
                {
                    double small_dist = (Math.Abs(hd) > Math.Abs(ld)) ? Math.Abs(ld) : Math.Abs(hd);
                    if (small_dist > radius)
                        return;
                }

                if (curLevel < Host.Depth)
                {
                    RightTree.ReportContent_possibleWithinRange(key, radius, ref train);
                    LeftTree.ReportContent_possibleWithinRange(key, radius, ref train);
                }
                else
                {
                    rwLocker.EnterReadLock();
                    try
                    {
                        for (int i = 0; i < PointList.Count; ++i)
                        {
                            train.Add(PointList[i].Value);
                        }
                    }
                    finally
                    {
                        rwLocker.ExitReadLock();
                    }
                }
            }

            public List<Cell> GetNeighborList()
            {
                return this.neighbors;
            }
        }
    }
}
